#include "troy.h"

Troy::Troy(QObject* parent) : Robot("Troy", parent){
    initComponents();
}

Troy::~Troy(){

}

void Troy::initComponents(){
    RobotSegment *newSegment, *body;
    RobotJointGroup *newJointGroup;
    Dimension3D dimensions;
    NameGroup names;
    //We are only going to use one name per component, so
    //set the last four to NULL_NAME
    names[1] = NULL_NAME;
    names[2] = NULL_NAME;
    names[3] = NULL_NAME;
    names[4] = NULL_NAME;

    //Body
    dimensions.x = 7.0;
    dimensions.y = 13.0;
    dimensions.z = 4.5;
    names[0] = T_BODY;
    body = new RobotSegment(names, dimensions, "Body", NULL, QVector3D(0,0,0), true);

    //Neck
    names[0] = T_NECK;
    newJointGroup = new RobotJointGroup(names, "Neck", body, JOINT_RADIUS,
                                        QVector3D(0,0.5,0),true);
    newJointGroup->addJoint(12, -90.0f, 90.0f, QVector3D(0, 1.0, 0), "Head Rotation");
    newJointGroup->addJoint(28, -60.0f, 90.0f, QVector3D(1.0, 0, 0), "Head Pitch");

    //Head
    dimensions.x = 8.0;
    dimensions.y = 5.0;
    dimensions.z = 2.0;
    names[0] = T_HEAD;
    newSegment = new RobotSegment(names, dimensions, "Head", newJointGroup,
                                  QVector3D(0,0.5,0), true);

    //Left shoulder
    names[0] = T_LEFT_SHOULDER;
    newJointGroup = new RobotJointGroup(names, "Left Shoulder", body, JOINT_RADIUS,
                                        QVector3D(5.5, -1.0, 0));
    //THE ORDER THAT THE SHOULDER JOINTS ARE ADDED ARE VERY IMPORTANT FOR THE ROTATION
    //We use a quaternion to represent the rotation, so we need to store the joints
    //in the opposite order (see an explanation of quaternion multiplication)
    newJointGroup->addJoint(8, -90.0f, 100.0f, QVector3D(0, 1.0, 0), "LH Rotation");
    newJointGroup->addJoint(1, -10.0f, 180.0f, QVector3D(0, 0, 1.0), "LS Abduction/Adduction");
    newJointGroup->addJoint(0, -20.0f, 180.0f, QVector3D(-1.0, 0, 0), "LS Flexion/Extension");

    //Left arm
    dimensions.x = 1.0;
    dimensions.y = 3.1;
    dimensions.z = 2.0;
    names[0] = T_LEFT_ARM;
    newSegment = new RobotSegment(names, dimensions, "Left Arm", newJointGroup);

    //Left Elbow
    names[0] = T_LEFT_ELBOW;
    newJointGroup = new RobotJointGroup(names, "Left Elbow", newSegment);
    newJointGroup->addJoint(9, -87.0f, 105.0f, QVector3D(0, 0, 1.0), "Left Elbow");

    //Left forearm, which is a child of the left elbow
    dimensions.x = 1.0;
    dimensions.y = 5.1;
    dimensions.z = 2.0;
    names[0] = T_LEFT_FOREARM;
    newSegment = new RobotSegment(names, dimensions, "Left Forearm", newJointGroup);

    //Right shoulder
    names[0] = T_RIGHT_SHOULDER;
    newJointGroup = new RobotJointGroup(names, "Right Shoulder", body, JOINT_RADIUS,
                                        QVector3D(-5.5, -1.0, 0));
    newJointGroup->addJoint(24, -90.0f, 100.0f, QVector3D(0, -1.0, 0), "RH Rotation");
    newJointGroup->addJoint(17, -10.0f, 180.0f, QVector3D(0, 0, -1.0), "RS Abduction/Adduction");
    newJointGroup->addJoint(16, -20.0f, 180.0f, QVector3D(-1.0, 0, 0), "RS Flexion/Extension");

    //Right arm
    dimensions.x = 1.0;
    dimensions.y = 3.1;
    dimensions.z = 2.0;
    names[0] = T_RIGHT_ARM;
    newSegment = new RobotSegment(names, dimensions, "Right Arm", newJointGroup);

    //Right elbow
    names[0] = T_RIGHT_ELBOW;
    newJointGroup = new RobotJointGroup(names, "Right Elbow", newSegment);
    newJointGroup->addJoint(25, -87.0f, 105.0f, QVector3D(0, 0, -1.0), "Right Elbow");

    //Right forearm, which is a child of the right elbow
    dimensions.x = 1.0;
    dimensions.y = 5.1;
    dimensions.z = 2.0;
    names[0] = T_RIGHT_FOREARM;
    newSegment = new RobotSegment(names, dimensions, "Right Forearm", newJointGroup);

    _rootComponent = body;
}

QSharedPointer<RobotPose> Troy::robotPose() const {
    QSharedPointer<TroyPose> troy(new TroyPose());
    foreach (int jointID, jointIDs()) {
        troy->setJointAngle(jointID, jointAngle(jointID));
    }
    return troy.dynamicCast<RobotPose>();
}
